/*
 * my_camera.c
 *
 *  Created on: Feb 19, 2024
 *      Author: 86138
 */
#include "zf_common_headfile.h"
#define HEIGHT 120
#define WIDTH 188

// �Ӿ�������ʼ��
uint8_t col = 188, row = 120, _row = 120, height = 120, width = 188;
uint8_t image_threshold[120][188] = {0};
uint8_t mode = 0;
uint8_t flag1 = 0;
uint8_t flag2 = 0;
float seed_delta = 0, seed_delta_ahead = 0;
uint8_t seed_left[120] = {0};
uint8_t seed_right[120] = {0};
uint8_t middle_line[120] = {0};
// uint8_t garage_out_flag = 0, garage_in_flag = 1;

uint8_t zuodiuxian_cnt = 0; // ���м��ζ���
uint8_t zuodiuxian_flag = 0;
uint8_t zuodiuxian[HEIGHT] = {0};
uint8_t zuodiuxian_start[HEIGHT] = {0};

uint8_t youdiuxian_cnt = 0; // ���м��ζ���
uint8_t youdiuxian_flag = 0;
uint8_t youdiuxian[HEIGHT] = {0};
uint8_t youdiuxian_start[HEIGHT] = {0};

uint8_t middle_line_end = 29;
uint8_t dec_right = 0,inc_right = 0;
uint8_t test1 = 0, right_circle_flag = 0;
uint8_t length[188] = {0};
uint8_t longest = 0;
uint8_t _longest = 0;
uint8_t white_col = 0;

//斑马线
uint8_t garage_out_flag = 0, garage_in_flag = 1;


void adapt_threshold(void)
{
  uint16_t pixelCount[256] = {0};
  float pixelPro[256] = {0};
  uint16_t i, j = 0;
  uint16_t pixelSum = col * _row / 4;
  uint8_t threshold = 0;
  uint32_t gray_sum = 0;
  float w0 = 0, w1 = 0, u0tmp = 0, u1tmp = 0, u0 = 0, u1 = 0, u = 0, deltaTmp = 0, deltaMax = 0;

  for (i = row - _row; i < row; i += 2)
  {
    for (j = 0; j < col; j += 2)
    {
      pixelCount[mt9v03x_image[i][j]]++;
      gray_sum += mt9v03x_image[i][j];
    }
  }

  for (i = 0; i <= 255; i++)
  {
    pixelPro[i] = (float)pixelCount[i] / pixelSum;
  }

  for (j = 0; j <= 255; j++)
  {
    w0 += pixelPro[j];
    u0tmp += j * pixelPro[j];
    w1 = 1 - w0;
    u = (float)gray_sum / pixelSum;
    u1tmp = u - u0tmp;

    u0 = u0tmp / w0;
    u1 = u1tmp / w1;

    //    u = u0tmp + u1tmp;
    deltaTmp = w0 * (u0 - u) * (u0 - u) + w1 * (u1 - u) * (u1 - u);
    if (deltaTmp > deltaMax)
    {
      deltaMax = deltaTmp;
      threshold = j;
    }
    if (deltaTmp < deltaMax)
    {
      break;
    }
  }

  for (i = 0; i < row; i++)
  {
    for (j = 0; j < col; j++)
    {
      if (mt9v03x_image[i][j] > threshold)
      {
        image_threshold[i][j] = 255;
      }
      else
      {
        image_threshold[i][j] = 0;
      }
    }
  }
}


void detect_seed(uint8_t _height)
{
    white_col = 0;

    for (uint8_t i = 70, m = 1; i < 120; i += m) {
        length[i]=0;
      for (uint8_t j = height, n = 1; j > n; j -= n) {
        if (image_threshold[j][i] == 255) {
          length[i]++;
          if (image_threshold[j - 1][i] == 0) {
            break;
          }
        }
      }
      if(length[i] >115)
      {
          white_col++;
      }
    }

//    for (uint8_t i = 70, m = 1; i < 120; i += m) {
//      if (length[i] > longest) {
//        longest = length[i];
//        _longest = i;
//      }
//    }






  uint8_t last_col = 1;
  uint8_t x = 0;
  uint8_t y = 0;
  uint8_t left_cnt = 0;
  uint8_t right_cnt = 0;

  for (y = row - 1; y > 0; y--)
  {
    if (y == _height)
    {
      x = 94;
    }
    else
    {
      x = last_col;
    }

    for (left_cnt = x; left_cnt > 0; left_cnt--)
    {
      uint8_t left_delta = image_threshold[y][left_cnt] - image_threshold[y][left_cnt - 1];

      if (left_delta > 200)
      {
        seed_left[y] = left_cnt;
        zuodiuxian_flag = 0;
        break;
      }
      else
      {
        seed_left[y] = 0;
        if (left_cnt == 1)
        {
          zuodiuxian_flag = 1;
          if (y < _height && zuodiuxian_flag == 1 && seed_left[y] == 0)
          {
            if (y == height - 1)
            {
              zuodiuxian_cnt = 1;
              zuodiuxian_start[zuodiuxian_cnt] = y;
              zuodiuxian[zuodiuxian_cnt]++;
            }
            else if (zuodiuxian_cnt == 0 && seed_left[y + 1] != 0)
            {
              zuodiuxian_cnt++;
              zuodiuxian_start[zuodiuxian_cnt] = y;
              zuodiuxian[zuodiuxian_cnt]++;
            }
            else if (zuodiuxian_cnt != 0 && seed_left[y + 1] >= 2 && abs(zuodiuxian_start[zuodiuxian_cnt] - zuodiuxian[zuodiuxian_cnt] - y) < 3)
            {
              zuodiuxian_cnt++;
              zuodiuxian_start[zuodiuxian_cnt] = y;
              zuodiuxian[zuodiuxian_cnt]++;
            }
            else if (zuodiuxian_cnt != 0 && seed_left[y + 1] != 0 && abs(zuodiuxian_start[zuodiuxian_cnt] - zuodiuxian[zuodiuxian_cnt] - y) != 0)
            {
              zuodiuxian_cnt++;
              zuodiuxian_start[zuodiuxian_cnt] = y;
              zuodiuxian[zuodiuxian_cnt]++;
            }
            else if (seed_left[y + 1] >= 0 && seed_left[y + 1] <= 2)
            {
              zuodiuxian[zuodiuxian_cnt]++;
            }
            else if (zuodiuxian_cnt == 1 && zuodiuxian_start[1] == 115 && zuodiuxian[1] < 10)
            {
              zuodiuxian_cnt = 0;
            }
          }
        }
      }
    }

    for (right_cnt = x; right_cnt < width - 1; right_cnt++)
    {
      int right_delta = image_threshold[y][right_cnt] - image_threshold[y][right_cnt + 1];
      if (right_delta > 200)
      {
        seed_right[y] = right_cnt;
        youdiuxian_flag = 0;
        break;
      }
      else
      {
        seed_right[y] = width - 1;
        if (right_cnt == width - 2)
        {
          youdiuxian_flag = 1;
          if (y < _height && youdiuxian_flag == 1 && seed_right[y] == width - 1)
          {
            if (y == height - 1)
            {
              youdiuxian_cnt = 1;
              youdiuxian_start[youdiuxian_cnt] = y;
              youdiuxian[youdiuxian_cnt]++;
            }
            else if (youdiuxian_cnt == 0 && seed_right[y + 1] != width - 1)
            {
              youdiuxian_cnt++;
              youdiuxian_start[youdiuxian_cnt] = y;
              youdiuxian[youdiuxian_cnt]++;
            }
            /*丢过线后，考虑到边界可能出现毛刺的影响，
            故而在下一次丢线会考虑上次丢线的终点，和这次疑似丢线的起点的距离，
            根据距离大小使用不同的判断条件，再来判断丢线是否继续*/
            else if (youdiuxian_cnt != 0 && seed_right[y + 1] <= width - 3 && abs(youdiuxian_start[youdiuxian_cnt] - youdiuxian[youdiuxian_cnt] - y) < 3)
            {
              youdiuxian_cnt++;
              youdiuxian_start[youdiuxian_cnt] = y;
              youdiuxian[youdiuxian_cnt]++;
            }
            else if (youdiuxian_cnt != 0 && seed_right[y + 1] != width - 1 && abs(youdiuxian_start[youdiuxian_cnt] - youdiuxian[youdiuxian_cnt] - y) >= 3)
            {
              youdiuxian_cnt++;
              youdiuxian_start[youdiuxian_cnt] = y;
              youdiuxian[youdiuxian_cnt]++;
            }
            else if (seed_right[y + 1] >= width - 3 && seed_right[y + 1] <= width - 1)
            {
              youdiuxian[youdiuxian_cnt]++;
            }
          }
        }
      }
    }

    image_threshold[y][(seed_right[y] + seed_left[y]) / 2] = 0;

    x = (uint8_t)((seed_left[y] + seed_right[y]) / 2);

    if (y < 110 &&
        (youdiuxian_flag == 1 && zuodiuxian_flag == 1))
    {
      for (uint8_t i = y; i < y + 5; i++)
      {
        if (seed_left[i] != 0 && seed_right[i] != width - 1)
        {
          middle_line[y] = middle_line[i];
          break;
        }
      }
      middle_line[y] = x;
    }
    else
    {
      middle_line[y] = x;
    }

    last_col = middle_line[y];

    test1 = 1;
    // last_col = (uint8_t)((seed_left[y] + seed_right[y]) / 2);
    if ((y > 0 && y < 70 && image_threshold[y - 1][middle_line[y]] == 0)
        || y == 0)
    {
      // 一般中点的点都是黑色的，如果发现下一行的点突然变白，认为不是中点，跳出扫线
      // 记录这时候的y为中线的中点
      // 写法1：直接跳出中线的寻找
      middle_line_end = y;



//      uint8_t inc_num = 5;
//      uint8_t dec_num = 25;
      test1 = 2;
      inc_right = 0;
      dec_right = 0;
      if(white_col >20 && right_circle_flag == 0
              && (youdiuxian_cnt == 0 || youdiuxian_start[1]> 100 || youdiuxian_start[1]<30))//&& youdiuxian_cnt != 0)
      {
          RoundaboutGetArc(height-1, 2, 2,2);

      }
//这里写的是没找到圆环就进行找拐点和拉线，如果最后抛弃圆环，就把上面几行和下面的判断条件删了，只跑十字吧
      if(right_circle_flag == 0)
      {
          CheckTurningPoint_up();
        conect_line_branch();
      }

      break;
    }
  }

  //  seed_delta_ahead = (float)(seed_left[seed] + seed_right[seed] - seed_left[seed_ahead] - seed_right[seed_ahead]) / 2;
}


uint8_t RoundaboutGetArc(uint8_t _height, uint8_t status, uint8_t inc_num, uint8_t dec_num)
{
  uint8_t i = 0;
  uint8_t inc = 0, dec = 0, n = 0;
  switch (status)
  {
//     case 1:
//       for (i = 119; i > middle_line_end; i--) {
//         //没有丢线
//         if (seed_left[i] != 0 && seed_left[i + 1] != 0)
//         {
//           if (seed_left[i] == seed_left[i + 1]) {
//             n++;
//             continue;
//           }
//           if (seed_left[i] > seed_left[i + 1]) {
//             inc++;
//             inc += n;
//             n = 0;
//           }
//           else {
//             dec++;
//             dec += n;
//             n = 0;
//           }
//
//           /* 有弧线 */
//           if (inc > 20 && dec > 12) {
//             //* index = i + num;
//             return 1;
//           }
//         }
//         else {
//           inc = 0;
//           dec = 0; n = 0;
//         }
//       }
//
//       break;

  case 2:

    for (i = 119; i > middle_line_end ; i--)
    {
      if (seed_right[i] != width - 1 && seed_right[i + 1] != width - 1)
      {
        if (seed_right[i] == seed_right[i + 1])
        {
          n++;
          continue;
        }
        if (seed_right[i] > seed_right[i + 1])
        {
          inc++;
          inc += n;
          n = 0;
        }
        else
        {
          dec++;
          dec += n;
          n = 0;
        }
        inc_right = inc;
        dec_right = dec;

        /* 有弧线 */
        if (right_circle_flag == 0 && inc > 12 && dec > 20)
        {
          right_circle_flag = 100;
          //right_circle_flag = 1;
          return 1;
          break;
        }
      }
      else
      {
        inc = 0;
        dec = 0;
        n = 0;
      }
    }

    break;
  }

  return 0;
}

uint8_t leftTurningpoint[HEIGHT][2] = {0}; // ����ÿ�ζ��߶��˶���

uint8_t rightTurningpoint[HEIGHT][2] = {0}; // ����ÿ�ζ��߶��˶���

uint8_t left_tp_cnt = 0;
uint8_t right_tp_cnt = 0;
uint8_t tp_r = 0;
uint8_t tp_l = 0;

void CheckTurningPoint_up(void)
{
  for (uint8_t i = 110; i > (middle_line_end + 1 < 55 ? 55 : middle_line_end + 1); i--)
  {
    if (abs(seed_left[i - 1] - seed_left[i - 2]) > 7 && abs(seed_left[i] - seed_left[i + 1]) < 5 && abs(seed_left[i - 1] - seed_left[i]) < 5
        //&& seed_left[i - 1] > 5
    )
    {

      if (left_tp_cnt >= 1 &&
          abs(leftTurningpoint[left_tp_cnt - 1][1] - i) < 3)
      {
        continue;
      }

      if (seed_left[i - 1] - seed_left[i - 2] > 0)
      {
        if (i - 1 < middle_line_end)
        {
          continue;
        }
        tp_l = i - 1 > middle_line_end ? i - 1 : middle_line_end;
      }
      else if (seed_left[i - 1] - seed_left[i - 2] < 0)
      {
        if (i - 4 < middle_line_end)
        {
          continue;
        }
        tp_l = i - 3 > middle_line_end ? i - 3 : middle_line_end;
      }
      left_tp_cnt++;
      leftTurningpoint[left_tp_cnt][0] = seed_left[tp_l];
      leftTurningpoint[left_tp_cnt][1] = tp_l;
      // debug
      //          var j = seed_left[tp_l];
      //          while (j < (seed_left[tp_l] + 40 < 186 ? seed_left[tp_l] + 40 : 186)) {
      //            //Attr1[tp_l][j][1] = test(200);
      //            j++;
      //          }
    }
  }

  for (uint8_t i = 110; i > (middle_line_end + 1 < 55 ? 55 : middle_line_end + 1); i--)
  {
    if (abs(seed_right[i + 2] - seed_right[i + 1]) > 5 && abs(seed_right[i] - seed_right[i + 1]) < 7 && abs(seed_right[i - 1] - seed_right[i]) < 7
        //&& right_up_point[0] == 0
    )
    {

      if (right_tp_cnt >= 1 &&
          abs(rightTurningpoint[right_tp_cnt][1] - i) < 5)
      {
        i -= 4;
        right_tp_cnt--;
        continue;
      }

      if (seed_right[i + 2] - seed_right[i + 1] > 0)
      {
        if (i + 1 < middle_line_end)
        {
          continue;
        }
        tp_r = i + 1 > middle_line_end ? i + 1 : middle_line_end;
      }
      else if (seed_right[i + 2] - seed_right[i + 1] < 0)
      {
        if (i + 3 < middle_line_end)
        {
          continue;
        }
        tp_r = i + 3 > middle_line_end ? i + 3 : middle_line_end;
      }
      right_tp_cnt++;
      rightTurningpoint[right_tp_cnt][0] = seed_right[tp_r]; // x����
      rightTurningpoint[right_tp_cnt][1] = tp_r;             // y����

      // debug
      //          var j = seed_right[tp_r];
      //          while (j > (seed_right[tp_r] - 30 > 0 ? seed_right[tp_r] - 30 : 0)) {
      //            Attr1[tp_r][j][2] = test(200);
      //            j--;
      //          }
    }
  }
}

uint8_t crossroads_flag = 0;
uint8_t conect_line_flag = 0;
uint8_t conect_line_flag_left = 0;
uint8_t conect_line_flag_right = 0;
void conect_line_branch(void)
{
    //第一种情况 两边都有两个拐点，最常见的就是在过十字的时候
  if (left_tp_cnt == 2 && right_tp_cnt == 2)
  {

    uint8_t k_left = 0;
    uint8_t b_left = 0;
    uint8_t delta_x_left = 0;
    uint8_t delta_y_left = 0;

    delta_x_left = leftTurningpoint[left_tp_cnt][0] - leftTurningpoint[left_tp_cnt - 1][0];
    delta_y_left = leftTurningpoint[left_tp_cnt][1] - leftTurningpoint[left_tp_cnt - 1][1];

    if (delta_y_left != 0)
    {
      // //     Attr2[12] = 2;
      k_left = delta_x_left / delta_y_left;
      b_left = ((leftTurningpoint[left_tp_cnt][0] - k_left * leftTurningpoint[left_tp_cnt][1]) + (leftTurningpoint[left_tp_cnt - 1][0] - k_left * leftTurningpoint[left_tp_cnt - 1][1])) / 2;

      for (uint8_t i = leftTurningpoint[left_tp_cnt - 1][1]; i > leftTurningpoint[left_tp_cnt][1]; i--)
      {
        // Attr1[i][seed_left[i]][1] = 0;
        seed_left[i] = (uint8_t)(k_left * i + b_left);
        // Attr1[i][seed_left[i]][1] = test(220);
      }
    }

    uint8_t k_right = 0;
    uint8_t b_right = 0;
    uint8_t delta_x_right = 0;
    uint8_t delta_y_right = 0;

    delta_x_right = rightTurningpoint[right_tp_cnt][0] - rightTurningpoint[right_tp_cnt - 1][0];
    delta_y_right = rightTurningpoint[right_tp_cnt][1] - rightTurningpoint[right_tp_cnt - 1][1];

    if (delta_y_right != 0)
    {
      k_right = delta_x_right / delta_y_right;

      b_right = ((rightTurningpoint[right_tp_cnt][0] - k_right * rightTurningpoint[right_tp_cnt][1]) + (rightTurningpoint[right_tp_cnt - 1][0] - k_right * rightTurningpoint[right_tp_cnt - 1][1])) / 2;

      for (uint8_t i = rightTurningpoint[right_tp_cnt - 1][1]; i > rightTurningpoint[right_tp_cnt][1]; i--)
      {
        // Attr1[i][seed_right[i]][2] = 0;
        seed_right[i] = (uint8_t)(k_right * i + b_right);
        // Attr1[i][seed_right[i]][2] = test(220);
      }
    }

    conect_line_flag = 1;

    for (uint8_t i =
             (rightTurningpoint[right_tp_cnt - 1][1] >
                      leftTurningpoint[left_tp_cnt - 1][1]
                  ? rightTurningpoint[right_tp_cnt - 1][1]
                  : leftTurningpoint[left_tp_cnt - 1][1]);
         i >
         (rightTurningpoint[right_tp_cnt][1] < leftTurningpoint[left_tp_cnt][1]
              ? rightTurningpoint[right_tp_cnt][1]
              : leftTurningpoint[left_tp_cnt][1]);
         i--)
    {
      // Attr1[i][middle_line[i]][3] = 0;
      middle_line[i] = (uint8_t)((seed_left[i] + seed_right[i]) / 2);
      // Attr1[i][middle_line[i]][3] = 200;
    }
  }


 //第二种情况左边一个拐点，多段丢线
  uint8_t _choosen_diuixan_flag_left = 1;

  if (zuodiuxian_cnt >= 1)
  {
    uint8_t temp = zuodiuxian[1];
    for (uint8_t i = 1; i < zuodiuxian_cnt; i++)
    {

      if (temp < zuodiuxian[i + 1])
      {
        _choosen_diuixan_flag_left = i + 1;
        temp = zuodiuxian[i + 1];
      }
    }
  }

  if (conect_line_flag == 0 &&
      left_tp_cnt == 1 &&
      zuodiuxian_cnt >= 1 &&
      abs((zuodiuxian_start[_choosen_diuixan_flag_left] - zuodiuxian[_choosen_diuixan_flag_left]) - leftTurningpoint[1][1]) < 5)
      //丢线的第一个点减去丢线的距离，那就是这段丢线的终点，要保证这个终点和拐点靠得很近，那么显然就是一段丢线紧接着一个拐点，那么这时候就要拉线
  {
    // Attr2[6] = 1;
    leftTurningpoint[left_tp_cnt - 1][0] = 0;
    leftTurningpoint[left_tp_cnt - 1][1] = 115;

    float k_left = 0;
    float b_left = 0;
    uint8_t delta_x_left = 0;
    uint8_t delta_y_left = 0;

    delta_x_left = leftTurningpoint[left_tp_cnt][0] - leftTurningpoint[left_tp_cnt - 1][0];
    delta_y_left = leftTurningpoint[left_tp_cnt][1] - leftTurningpoint[left_tp_cnt - 1][1];

    if (delta_y_left != 0)
    {
      // //     Attr2[12] = 2;
      k_left = (float)delta_x_left / delta_y_left;
      b_left = (float)((leftTurningpoint[left_tp_cnt][0] - k_left * leftTurningpoint[left_tp_cnt][1]) + (leftTurningpoint[left_tp_cnt - 1][0] - k_left * leftTurningpoint[left_tp_cnt - 1][1])) / 2;

      for (uint8_t i = leftTurningpoint[left_tp_cnt - 1][1];
           i > leftTurningpoint[left_tp_cnt][1];
           i--)
      {

        seed_left[i] = (uint8_t)(k_left * i + b_left);
      }
    }

    conect_line_flag_left = 1;
  }


  //第3种情况右边一个拐点，多段丢线
  uint8_t _choosen_diuixan_flag_right = 1;

  if (youdiuxian_cnt >= 1)
  {
    uint8_t temp = youdiuxian[1];
    for (uint8_t i = 1; i < youdiuxian_cnt; i++)
    {

      if (temp < youdiuxian[i + 1])
      {
        _choosen_diuixan_flag_right = i + 1;
        temp = youdiuxian[i + 1];
      }
    }
  }

  if (conect_line_flag == 0 &&
      right_tp_cnt == 1 &&
      youdiuxian_cnt >= 1 &&
      abs((youdiuxian_start[_choosen_diuixan_flag_right] - youdiuxian[_choosen_diuixan_flag_right]) - rightTurningpoint[1][1]) < 5)
  {
    // Attr2[6] = 1;
    rightTurningpoint[right_tp_cnt - 1][0] = 186;
    rightTurningpoint[right_tp_cnt - 1][1] = height;

    float k_right = 0;
    float b_right = 0;
    float delta_x_right = 0;
    float delta_y_right = 0;

    delta_x_right = rightTurningpoint[right_tp_cnt][0] - rightTurningpoint[right_tp_cnt - 1][0];
    delta_y_right = rightTurningpoint[right_tp_cnt][1] - rightTurningpoint[right_tp_cnt - 1][1];

    if (delta_y_right != 0)
    {
      // //     Attr2[12] = 2;
      k_right = (float)delta_x_right / delta_y_right;
      b_right = ((rightTurningpoint[right_tp_cnt][0] - k_right * rightTurningpoint[right_tp_cnt][1]) + (rightTurningpoint[right_tp_cnt - 1][0] - k_right * rightTurningpoint[right_tp_cnt - 1][1])) / 2;

      for (uint8_t i = rightTurningpoint[right_tp_cnt - 1][1]; i > rightTurningpoint[right_tp_cnt][1]; i--)
      {
        seed_right[i] = (uint8_t)(k_right * i + b_right);
      }
    }
    conect_line_flag_right = 1;
  }


  //左边两个拐点,但是第二个拐点相当于没有用，而且第一个拐点前面有很多丢线
    if (left_tp_cnt > 1 && zuodiuxian_cnt > 0 && zuodiuxian_start[1] > 110
      && (leftTurningpoint[1][1] + zuodiuxian[1] > 110)) {

      leftTurningpoint[0][0] = 0;
      leftTurningpoint[0][1] = 115;

      uint8_t k_left = 0;
      uint8_t b_left = 0;
      uint8_t delta_x_left = 0;
      uint8_t delta_y_left = 0;

      delta_x_left = leftTurningpoint[1][0] - leftTurningpoint[0][0];
      delta_y_left = leftTurningpoint[1][1] - leftTurningpoint[0][1];

      if (delta_y_left != 0) {
        // //     Attr2[12] = 2;
        k_left = delta_x_left / delta_y_left;
        b_left = ((leftTurningpoint[1][0] - k_left * leftTurningpoint[1][1])
          + (leftTurningpoint[0][0] - k_left * leftTurningpoint[0][1])) / 2;
        // Attr2[7] = k_left;
        // Attr2[8] = b_left;
        for (uint8_t i = leftTurningpoint[0][1];
          i > leftTurningpoint[1][1];
          i--) {
          //Attr1[i][seed_left[i]][1] = 0;
          seed_left[i] = (uint8_t)(k_left * i + b_left);
          //Attr1[i][seed_left[i]][1] = test(220);
        }
      }

      conect_line_flag_left = 1;
    }


    //右边两个拐点
    if (right_tp_cnt == 2) {

        uint8_t k_right = 0;
        uint8_t b_right = 0;
        uint8_t delta_x_right = 0;
        uint8_t delta_y_right = 0;

      delta_x_right = rightTurningpoint[right_tp_cnt][0] - rightTurningpoint[right_tp_cnt - 1][0];
      delta_y_right = rightTurningpoint[right_tp_cnt][1] - rightTurningpoint[right_tp_cnt - 1][1];

      if (delta_y_right != 0) {
        // //     Attr2[12] = 2;
        k_right = delta_x_right / delta_y_right;
        b_right = ((rightTurningpoint[right_tp_cnt][0] - k_right * rightTurningpoint[right_tp_cnt][1])
          + (rightTurningpoint[right_tp_cnt - 1][0] - k_right * rightTurningpoint[right_tp_cnt - 1][1])) / 2;

        for (uint8_t i = rightTurningpoint[right_tp_cnt - 1][1]; i > rightTurningpoint[right_tp_cnt][1]; i--) {
          //Attr1[i][seed_right[i]][2] = 0;
          seed_right[i] = (uint8_t)(k_right * i + b_right);
          //Attr1[i][seed_right[i]][2] = test(220);
        }
      }


      conect_line_flag_right = 1;
    }

    //右边两个拐点
    if (left_tp_cnt == 2) {
        uint8_t k_left = 0;
        uint8_t b_left = 0;
        uint8_t delta_x_left = 0;
        uint8_t delta_y_left = 0;

      delta_x_left = leftTurningpoint[left_tp_cnt][0] - leftTurningpoint[left_tp_cnt - 1][0];
      delta_y_left = leftTurningpoint[left_tp_cnt][1] - leftTurningpoint[left_tp_cnt - 1][1];

      if (delta_y_left != 0) {
        // //     Attr2[12] = 2;
        k_left = delta_x_left / delta_y_left;
        b_left = ((leftTurningpoint[left_tp_cnt][0] - k_left * leftTurningpoint[left_tp_cnt][1])
          + (leftTurningpoint[left_tp_cnt - 1][0] - k_left * leftTurningpoint[left_tp_cnt - 1][1])) / 2;
        // Attr2[7] = k_left;
        // Attr2[8] = b_left;
        for (uint8_t i = leftTurningpoint[left_tp_cnt - 1][1];
          i > leftTurningpoint[left_tp_cnt][1];
          i--) {
          //Attr1[i][seed_left[i]][1] = 0;
          seed_left[i] = (uint8_t)(k_left * i + b_left);
          //Attr1[i][seed_left[i]][1] = test(220);
        }
      }

      conect_line_flag_left = 1;
    }








  //之前只是拉了边线 现在是拉中线的步骤
  if (conect_line_flag_right && conect_line_flag_left)
  {
    for (uint8_t i =
             (rightTurningpoint[right_tp_cnt - 1][1] >
                      leftTurningpoint[left_tp_cnt - 1][1]
                  ? rightTurningpoint[right_tp_cnt - 1][1]
                  : leftTurningpoint[left_tp_cnt - 1][1]);
         i >
         (rightTurningpoint[right_tp_cnt][1] < leftTurningpoint[left_tp_cnt][1]
              ? rightTurningpoint[right_tp_cnt][1]
              : leftTurningpoint[left_tp_cnt][1]);
         i--)
    {
      middle_line[i] = (uint8_t)((seed_left[i] + seed_right[i]) / 2);
    }
  }

  if (conect_line_flag_left == 1 && conect_line_flag_right == 0)
  {
    for (uint8_t i = leftTurningpoint[left_tp_cnt - 1][1];
         i > leftTurningpoint[left_tp_cnt][1];
         i--)
    {
      middle_line[i] = (uint8_t)((seed_left[i] + seed_right[i]) / 2);
    }
  }

  if (conect_line_flag_left == 0 && conect_line_flag_right == 1)
  {
    for (uint8_t i = rightTurningpoint[right_tp_cnt - 1][1];
         i > rightTurningpoint[right_tp_cnt][1];
         i--)
    {
      middle_line[i] = (uint8_t)((seed_left[i] + seed_right[i]) / 2);
    }
  }
}

void delta(void)
{
    if(right_circle_flag == 0)
    {
        uint8_t seed = 80;
        uint8_t seed_ahead = 70;
        for (uint8_t i = seed_ahead; i < seed; i++)
        {
          seed_delta += 94 - ((float)(seed_left[i] + seed_right[i]) / 2);
        }
        seed_delta /= 10.0f;
        seed_delta_ahead = (float)(seed_left[seed] + seed_right[seed] - seed_left[seed_ahead] - seed_right[seed_ahead]) / 2;
    }
    else if(right_circle_flag)
    {
        right_circle_flag--;
        uint8_t seed = 80;
        uint8_t seed_ahead = 70;
        for (uint8_t i = seed_ahead; i < seed; i++)
        {
           seed_delta += 94 - (float)(seed_right[i]-60);
        }
        seed_delta /= 10.0f;
        seed_delta_ahead = (float)(seed_left[seed] + seed_right[seed] - seed_left[seed_ahead] - seed_right[seed_ahead]) / 2;

    }

}

void garage(void)
{
  uint8_t white_black = 1, black_white = 1, garage_count = 0, region = 0;
  for (uint8_t i = 60; i < 80; i++)
  {
    garage_count = 0;
    for (uint8_t j = 0; j < 187; j++)
    {

      if (image_threshold[i][j] == 255)
      {
        white_black = 1;
      }
      else
      {
        white_black = 0;
      }

      if (white_black != black_white)
      {
        black_white = white_black;
        garage_count++;
      }
    }

    if (garage_count > 15 && garage_count < 23)
    {
      region++;
    }

    if (region > 7)
    {
      garage_out_flag = 1;
      flag = 0;
      break;
    }
  }
}
